/* A simple server in the internet domain using TCP
   The port number is passed as an argument */
#include <stdlib.h>
#include <stdio.h>
#include <string.h>

#include "bhtpdvpls_structs.h"
#include "pslsimfunc.h"
#include "socket_header.h"
#include <opencv/cv.h>
#include <opencv/highgui.h>


#define DATA "POS1"

int main(int argc, char *argv[])
{
    if (argc < 5)
    {
        fprintf(stderr,"ERROR, no port provided\n");
        exit(1);
    }
    int enableRobs = 0;
    int enableCam = 1;

    const char * hostname	= argv[1];
    int 	 portno 	= atoi(argv[2]);

    const char * camhostname	= argv[3];
    int 	 camportno 	= atoi(argv[4]);

    int robotID = 4711;
    int n = 0;
    cvNamedWindow( "Hallo", CV_WINDOW_AUTOSIZE);
    while (1)
    {
        int robsock = -1;
        if (enableRobs)
            robsock   = connectSocket(hostname, portno);

        int camsock = -1;
        if (enableCam)
            camsock = connectSocket(camhostname, camportno);
        printf("Connected\n");
        while (1)
        {
            ComandPkg command;
            ObjektListe ol;
            if (enableRobs)
            {
                printf("send to   rob %s\n",  commandToStr[STATUS_REQUEST]);

                /*
                * wo ist der robot
                */
                command.cmd 		= STATUS_REQUEST;
                command.kord.objektId 	= robotID;
                if (!sendCmdPkg(robsock, command))
                {
                    printf("Disconnected\n");
                    break;
                }

                ComandPkg answer;
                n  = readin(robsock, (char*)&answer,  sizeof(answer));
                if (n == 0)
                {
                    printf("Disconnected\n");
                    break;
                }
                printf("recv from rob %s ",  commandToStr[answer.cmd]);
                printf("id%04d:%04dx:%04dy:%04dphi\n",
                       answer.kord.objektId, answer.kord.xkordinate_mm, answer.kord.ykordinate_mm, answer.kord.theta_grad);
            }
            /*
            * liste vom cam server hollen
            */
            /*
                        printf("send to   cam %s\n",  commandToStr[OBJECTS_REQUEST]);
                        if (!sendCmd(camsock, OBJECTS_REQUEST) )
                        {
                            printf("Disconnected\n");
                            break;
                        }
            */
            if (enableCam)
            {
                send(camsock, DATA,  strlen(DATA), 0);
                n  = readin(camsock, (char*)&ol,  sizeof(ObjektListe));
		printf("read ol\n");

                if (n == 0)
                {
                    printf("Disconnected\n");
                    break;
                }
                IplImage * cambild = (IplImage*)malloc(sizeof(IplImage));
                n  = readin(camsock, (char*)cambild,  sizeof(IplImage));
              	 printf("read image header\n");
                IplImage * tmp = cvCreateImage(cvSize(cambild->width, cambild->height), cambild->depth, cambild->nChannels);
                n  = readin(camsock, tmp->imageData,  tmp->imageSize);
                
              	printf("read image\n");
                cvShowImage(   "Hallo", tmp);
                // cvWaitKey(0);
                cvWaitKey(10);
                printf("bild x%d y%d\n", cvGetSize(tmp).width, cvGetSize(tmp).height);
                cvReleaseImage(&tmp);
                free(cambild);
            }
            if (enableRobs)
            {
                /*
                 * liste mit den objekten senden
                 */
                printf("send to   rob %s\n",  commandToStr[OBJECTS_UPDATE]);
                if (!sendCmd(robsock, OBJECTS_UPDATE))
                {
                    printf("Disconnected\n");
                    break;
                }

                n  = send(robsock, (char*)&ol,  sizeof(ObjektListe), 0);
                if (n == 0)
                {
                    printf("Disconnected\n");
                    break;
                }

                /*
                 * stop senden
                 */
                printf("send to   rob %s\n",  commandToStr[STOP_COMMAND]);
                if (!sendCmd(robsock, STOP_COMMAND))
                {
                    printf("Disconnected\n");
                    break;
                }

                /*
                 * ziel vorgeben
                 */
                printf("send to   rob %s\n",  commandToStr[DESTINATION_COMMAND]);
                memset(&command, 0, sizeof(ComandPkg));
                command.cmd = DESTINATION_COMMAND;
                n  = send(robsock, (char*)&command,  sizeof(ComandPkg), 0);
                if (n == 0)
                {
                    printf("Disconnected\n");
                    break;
                }

                /*
                 * start senden
                 */
                printf("send to   rob %s\n",  commandToStr[START_COMMAND]);
                if (!sendCmd(robsock, START_COMMAND))
                {
                    printf("Disconnected\n");
                    break;
                }
            }
            //usleep(1000*1000);
        }
        close(robsock);
    }
    return 0;
}
